#include "Rotation.h"
#include "Quaternion.h"
#include "Vect3.h"

#include <cmath>
#include <cstdlib>
#include <iostream>

using namespace RSIM;

////////////////////////////////////////////////////////////////////////

Rotation::Rotation():Mat3x3(){
        this->Identity();
}

////////////////////////////////////////////////////////////////////////

void Rotation::operator=(const Rotation& Rot){
        const double *Q = Rot.Ptr();
        this->data_[0] = Q[0];	this->data_[1] = Q[1];	this->data_[2] = Q[2];
        this->data_[3] = Q[3];	this->data_[4] = Q[4]; 	this->data_[5] = Q[5];
        this->data_[6] = Q[6];	this->data_[7] = Q[7]; 	this->data_[8] = Q[8];
}

////////////////////////////////////////////////////////////////////////

void Rotation::operator=(const Mat3x3& M){
        const double *Q = M.Ptr();
        this->data_[0] = Q[0];	this->data_[1] = Q[1];	this->data_[2] = Q[2];
        this->data_[3] = Q[3];	this->data_[4] = Q[4]; 	this->data_[5] = Q[5];
        this->data_[6] = Q[6];	this->data_[7] = Q[7]; 	this->data_[8] = Q[8];
}

////////////////////////////////////////////////////////////////////////

Rotation::Rotation(const Rotation& Rot):Mat3x3(Rot){}

////////////////////////////////////////////////////////////////////////

Rotation::Rotation(const Mat3x3& M):Mat3x3(M){}

////////////////////////////////////////////////////////////////////////

Rotation::Rotation(const double& ang1, const double& ang2, const double& ang3, const char *ch){
        if(!strcmp(ch,"Body123")){
                this->setRotationBody123(Vect3(ang1,ang2,ang3));
        }
        else if(!strcmp(ch,"Space123")){
                this->setRotationSpace123(Vect3(ang1,ang2,ang3));
        }
        else{
                cout<<"\nRotation::Rotation(const double&, const double&, const double&, const char *), \
                unexpected argument. options are \"Body123\" or \"Space123\""<<endl;
                exit(0);
        }
}

////////////////////////////////////////////////////////////////////////

Rotation::Rotation(const Quaternion& e){
        const double *dataptr = e.Ptr();
        
        const double& e0 = dataptr[0];
        const double& e1 = dataptr[1];
        const double& e2 = dataptr[2];
        const double& e3 = dataptr[3];
        
        this->data_[0] = e1*e1 - e2*e2 - e3*e3 + e0*e0;	        this->data_[1] = 2*(e1*e2 - e3*e0);			this->data_[2]= 2*(e1*e3 + e2*e0);
        this->data_[3] = 2*(e1*e2 + e3*e0);			this->data_[4] = -e1*e1 + e2*e2 - e3*e3 + e0*e0;	this->data_[5] = 2*(e2*e3 - e1*e0);
        this->data_[6] = 2*(e1*e3 - e2*e0);			this->data_[7] = 2*(e2*e3 + e1*e0);			this->data_[8] = -e1*e1 - e2*e2 + e3*e3 + e0*e0;
}

////////////////////////////////////////////////////////////////////////

void Rotation::setRotationZAxis(const double& angle){
        
        const double& s3 = sin(angle);
        const double& c3 = cos(angle);
        
        this->data_[0] = c3;	this->data_[1] = -s3;	this->data_[2] = 0.0;
        this->data_[3] = s3;	this->data_[4] = c3;	this->data_[5] = 0.0;
        this->data_[6] = 0.0;	this->data_[7] = 0.0; this->data_[8] = 1.0;
}

////////////////////////////////////////////////////////////////////////
// Equations taken from appendix 1 of \cite{Kane} (See end of this file)

void Rotation::setRotationBody123(const Vect3& q){
        const double *ptr = q.Ptr();
        
        const double& s1 = sin(ptr[0]);
        const double& s2 = sin(ptr[1]);
        const double& s3 = sin(ptr[2]);
        
        const double& c1 = cos(ptr[0]);
        const double& c2 = cos(ptr[1]);
        const double& c3 = cos(ptr[2]);
        
        this->data_[0] = c2*c3;		this->data_[1] = -c2*s3;		this->data_[2] = s2;
        this->data_[3] = s1*s2*c3 + s3*c1; 	this->data_[4] = -s1*s2*s3 + c3*c1; 	this->data_[5] = -s1*c2;
        this->data_[6] = -c1*s2*c3 + s3*s1;	this->data_[7] = c1*s2*s3 + c3*s1;	this->data_[8] = c1*c2;
}

////////////////////////////////////////////////////////////////////////
// Equations taken from appendix 1 of \cite{Kane} (See end of file)
void Rotation::setRotationSpace123(const Vect3& q){
        
        const double *ptr = q.Ptr();
        const double& s1 = sin(ptr[0]);
        const double& s2 = sin(ptr[1]);
        const double& s3 = sin(ptr[2]);
        
        const double& c1 = cos(ptr[0]);
        const double& c2 = cos(ptr[1]);
        const double& c3 = cos(ptr[2]);
        
        this->data_[0] = c2*c3;	this->data_[1] = s1*s2*c3 - s3*c1;	this->data_[2]= c1*s2*c3 + s3*s1;
        this->data_[3] = c2*s3;	this->data_[4] = s1*s2*s3 + c3*c1;	this->data_[5] = c1*s2*s3 - c3*s1;
        this->data_[6] = -s2;		this->data_[7] = s1*c2;		this->data_[8] = c1*c2;
}

////////////////////////////////////////////////////////////////////////
// Taken from \cite{Farid}
// Equation 2.94
void Rotation::setRotationFromQuaternion(const Quaternion& e){
        const double *dataptr = e.Ptr();
        
        const double& e0 = dataptr[0];
        const double& e1 = dataptr[1];
        const double& e2 = dataptr[2];
        const double& e3 = dataptr[3];
        
        this->data_[0] = e1*e1 - e2*e2 - e3*e3 + e0*e0;	        this->data_[1] = 2*(e1*e2 - e3*e0);			this->data_[2]= 2*(e1*e3 + e2*e0);
        this->data_[3] = 2*(e1*e2 + e3*e0);			this->data_[4] = -e1*e1 + e2*e2 - e3*e3 + e0*e0;	this->data_[5] = 2*(e2*e3 - e1*e0);
        this->data_[6] = 2*(e1*e3 - e2*e0);			this->data_[7] = 2*(e2*e3 + e1*e0);			this->data_[8] = -e1*e1 - e2*e2 + e3*e3 + e0*e0;
}

////////////////////////////////////////////////////////////////////////
// Taken from \cite{Farid}
// Equation 2.94
void Rotation::setRotationFromQuaternion(const double *q){
                
        this->data_[0] = q[1]*q[1] - q[2]*q[2] - q[3]*q[3] + q[0]*q[0];
        this->data_[1] = 2*(q[1]*q[2] - q[3]*q[0]);
        this->data_[2] = 2*(q[1]*q[3] + q[2]*q[0]);
        
        this->data_[3] = 2*(q[1]*q[2] + q[3]*q[0]);
        this->data_[4] = -q[1]*q[1] + q[2]*q[2] - q[3]*q[3] + q[0]*q[0];
        this->data_[5] = 2*(q[2]*q[3] - q[1]*q[0]);
        
        this->data_[6] = 2*(q[1]*q[3] - q[2]*q[0]);
        this->data_[7] = 2*(q[2]*q[3] + q[1]*q[0]);
        this->data_[8] = -q[1]*q[1] - q[2]*q[2] + q[3]*q[3] + q[0]*q[0];
}

////////////////////////////////////////////////////////////////////////

// @article{Kane,
// title={{Spacecraft dynamics}},
// author={Kane, TR and Likins, PW and Levinson, DA},
// year={1983}
// }

// @book{Farid,
// title={{Fundamentals of multibody dynamics: theory and applications}},
// author={Amirouche, F.M.L.},
// year={2006},
// publisher={Birkhauser}
// }